#include <iostream>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include<math.h>
 
 
using namespace cv;
using namespace std;
//手眼标定过程：
//1.获取18组机械臂工具尖点在基坐标系下的位姿参数，以及对应姿态的相机外参，分别放入两个数组。
//2.将两组参数转换成4X4的齐次矩阵，并再利用分解函数，将其分解为3X3的旋转矩阵和3X1的平移矩阵。
//3.以上一步分解得到的4组数据作为手眼标定函数的输入，输出位手眼矩阵的旋转矩阵和平移矩阵。
//4.将手眼矩阵的旋转和平移矩阵通过合并函数合并为4X4的齐次矩阵，即为所求到的手眼矩阵。





//标定板到相机：平移向量，旋转矩阵
Mat_<double> CalPoseTran1 = (Mat_<double>(3, 1) <<   0.149921, -0.0410865, 0.722964);
Mat_<double> CalPoseTran2 = (Mat_<double>(3, 1) <<  0.149021, -0.224614, 0.565741);
Mat_<double> CalPoseTran3 = (Mat_<double>(3, 1) <<  0.148257,  0.242246, 0.758951);
Mat_<double> CalPoseTran4 = (Mat_<double>(3, 1) << -0.281636, -0.0268598, 0.664437);
Mat_<double> CalPoseTran5 = (Mat_<double>(3, 1) << -0.0448419, -0.109237, 0.600128);
// Mat_<double> CalPoseTran6 = (Mat_<double>(3, 1) << -0.0448419, -0.109237, 0.600128);
// Mat_<double> CalPoseTran7 = (Mat_<double>(3, 1) << 0.220894, -0.00267802, 0.619714);
// Mat_<double> CalPoseTran8 = (Mat_<double>(3, 1) << -0.25002, 0.0343479, 0.597529);
 
Mat_<double> CalPoseRot1 = (Mat_<double>(3, 3) <<
 0.0771805,   0.991317,   0.106462, 
 -0.989865,  0.0889555,  -0.110695, 
 -0.119205, -0.0968394,   0.988136   
);
Mat_<double> CalPoseRot2 = (Mat_<double>(3, 3) <<
 0.0665489,   0.993833,  0.0886932,   
 -0.971327,  0.0848617,  -0.222088,  
 -0.228245, -0.0713704,   0.970984   
);
Mat_<double> CalPoseRot3 = (Mat_<double>(3, 3) <<
 0.077636,  0.990902,  0.109933,  
-0.992022, 0.0657938,  0.107533,  
0.0993218, -0.117404,  0.988105  
);
Mat_<double> CalPoseRot4 = (Mat_<double>(3, 3) <<
 -0.185311,   0.951373,  -0.246067,  
 -0.965087,  -0.223366,  -0.136805, 
 -0.185116,   0.212124,    0.95955   
);
Mat_<double> CalPoseRot5 = (Mat_<double>(3, 3) <<
    0.1483,   0.971399,   0.185451,   
 -0.981857,   0.167033, -0.0897631,  
 -0.118172,  -0.168775,   0.978545    
);
// Mat_<double> CalPoseRot6 = (Mat_<double>(3, 3) <<
// -0.0190686,   0.961295,   -0.27486, 
//  -0.725344,   0.175905,   0.665532,   
//   0.688122,   0.212059,   0.693916   
// );
// Mat_<double> CalPoseRot7 = (Mat_<double>(3, 3) <<
//  -0.0625555,    0.952248,    0.298848,    
//   -0.774375,   -0.235211,    0.587383, 
//    0.629626,   -0.194677,    0.752111    
// );
// Mat_<double> CalPoseRot8 = (Mat_<double>(3, 3) <<
// -0.445483,  0.895056, 0.0204989,  
// -0.719486, -0.371537,  0.586771, 
//  0.532809,  0.246648,  0.809493  
// );
 





//末端坐标系在基坐标系下的转换：平移向量，旋转矩阵
Mat_<double> ToolPoseTran1 = (Mat_<double>(3, 1) << -0.156759, 0.0861505,    0.530322);
Mat_<double> ToolPoseTran2 = (Mat_<double>(3, 1) <<  -0.15668, 0.0824832,   0.39381);
Mat_<double> ToolPoseTran3 = (Mat_<double>(3, 1) << -0.109925, 0.0938176, 0.563093);
Mat_<double> ToolPoseTran4 = (Mat_<double>(3, 1) << -0.132792, 0.0964829, 0.518623);
Mat_<double> ToolPoseTran5 = (Mat_<double>(3, 1) << -0.149937, 0.0844931, 0.387012);
// Mat_<double> ToolPoseTran6 = (Mat_<double>(3, 1) << -0.00828951, -0.0185944, 0.51828);
// Mat_<double> ToolPoseTran7 = (Mat_<double>(3, 1) << -0.00570234, -0.0068036, 0.556713);
// Mat_<double> ToolPoseTran8 = (Mat_<double>(3, 1) << -0.0210145, -0.0230521, 0.551294);
 
 
Mat_<double> ToolPoseRot1 = (Mat_<double>(3, 3) <<
  -0.978074,    0.208226, -0.00358895,   
  -0.208244,   -0.978061,  0.00554566,   
-0.00235546,  0.00617144,    0.999978    
);
Mat_<double> ToolPoseRot2 = (Mat_<double>(3, 3) <<
  -0.973781,    0.204761,   0.0991127,   
  -0.203988,   -0.978808,    0.017981,   
   0.100694, -0.00270826,    0.994914     
);
Mat_<double> ToolPoseRot3 = (Mat_<double>(3, 3) <<
 -0.956985,   0.197329,  -0.212699,  
 -0.192902,  -0.980337, -0.0415811,  
 -0.216722,  0.0012375,   0.976233   
);
Mat_<double> ToolPoseRot4 = (Mat_<double>(3, 3) <<
 -0.966136,   -0.25726,  0.0199582,  
  0.257348,  -0.966317, 0.00196764,  
 0.0187798, 0.00703723,   0.999799   
);
Mat_<double> ToolPoseRot5 = (Mat_<double>(3, 3) <<
  -0.955293,    0.295555, -0.00791095,
   -0.29555,   -0.955325, -0.00183463,   
-0.00809977, 0.000585475,    0.999967    
);
// Mat_<double> ToolPoseRot6 = (Mat_<double>(3, 3) <<
//    0.995716,  -0.0908636,     0.01715, 
//   0.0919929,     0.95464,   -0.283197,  
//  0.00936021,    0.283561,    0.958908     
// );
// Mat_<double> ToolPoseRot7 = (Mat_<double>(3, 3) <<
//    0.988089,  -0.0610147,    -0.14127, 
//   0.0994886,    0.953657,    0.283971,  
//    0.117396,   -0.294644,    0.948369   
// );
// Mat_<double> ToolPoseRot8 = (Mat_<double>(3, 3) <<
//   0.929797,   0.340001,  -0.140986, 
//   -0.33429,   0.940354,  0.0631205, 
//   0.154037, -0.0115592,   0.987997   
// );



//旋转矩阵R和平移向量T合并为4X4的齐次矩阵
Mat R_T2RT(Mat& R, Mat& T)
{
	Mat RT;
	Mat_<double> R1 = (Mat_<double>(4, 3) << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
		R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
		R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
		0.0, 0.0, 0.0);
	Mat_<double> T1 = (cv::Mat_<double>(4, 1) << T.at<double>(0, 0), T.at<double>(1, 0), T.at<double>(2, 0), 1.0);
	hconcat(R1, T1, RT);//C=A+B左右拼接
	return RT;
}

//4X4的齐次矩阵转换为旋转矩阵R和平移向量T
void RT2R_T(Mat& RT, Mat& R, Mat& T)
{
	Rect R_rect(0, 0, 3, 3);
	Rect T_rect(3, 0, 1, 3);
	R = RT(R_rect);
	T = RT(T_rect);
}



//打印输出矩阵
void print(vector<Mat>& v)
{
	for (vector<Mat>::iterator it = v.begin(); it != v.end(); it++)
	{
		cout << *it << " ";
		cout << endl;
	}
}
 
int main()
{
	//定义手眼标定矩阵
	vector<Mat> R_gripper2base; // Tbg
	vector<Mat> T_gripper2base;
	vector<Mat> R_target2cam; // Tct
	vector<Mat> T_target2cam;


	R_target2cam.push_back(CalPoseRot1);
	R_target2cam.push_back(CalPoseRot2);
	R_target2cam.push_back(CalPoseRot3);
	R_target2cam.push_back(CalPoseRot4);
	R_target2cam.push_back(CalPoseRot5);
	// R_target2cam.push_back(CalPoseRot6);
	// R_target2cam.push_back(CalPoseRot7);
	// R_target2cam.push_back(CalPoseRot8);
 
	R_gripper2base.push_back(ToolPoseRot1);
	R_gripper2base.push_back(ToolPoseRot2);
	R_gripper2base.push_back(ToolPoseRot3);
	R_gripper2base.push_back(ToolPoseRot4);
	R_gripper2base.push_back(ToolPoseRot5);
	// R_gripper2base.push_back(ToolPoseRot6);
	// R_gripper2base.push_back(ToolPoseRot7);
	// R_gripper2base.push_back(ToolPoseRot8);
 
	T_target2cam.push_back(CalPoseTran1);
	T_target2cam.push_back(CalPoseTran2);
	T_target2cam.push_back(CalPoseTran3);
	T_target2cam.push_back(CalPoseTran4);
	T_target2cam.push_back(CalPoseTran5);
	// T_target2cam.push_back(CalPoseTran6);
	// T_target2cam.push_back(CalPoseTran7);
	// T_target2cam.push_back(CalPoseTran8);
 
	T_gripper2base.push_back(ToolPoseTran1);
	T_gripper2base.push_back(ToolPoseTran2);
	T_gripper2base.push_back(ToolPoseTran3);
	T_gripper2base.push_back(ToolPoseTran4);
	T_gripper2base.push_back(ToolPoseTran5);
	// T_gripper2base.push_back(ToolPoseTran6);
	// T_gripper2base.push_back(ToolPoseTran7);
	// T_gripper2base.push_back(ToolPoseTran8);
 
	Mat R_cam2gripper = (Mat_<double>(3, 3));
	Mat T_cam2gripper = (Mat_<double>(3, 1));
 
 
	Mat Hcg;//定义相机camera到末端grab的位姿矩阵
 
	//使用calibrateHandEye函数进行手眼标定
	calibrateHandEye(R_gripper2base, T_gripper2base, R_target2cam, T_target2cam, R_cam2gripper, T_cam2gripper, CALIB_HAND_EYE_TSAI);
	cout << "手眼矩阵为：" << endl;
	Hcg = R_T2RT(R_cam2gripper, T_cam2gripper);//矩阵合并
	print(Hcg);
 
	std::vector<cv::Mat> vecHg, vecHc;
 
	cv::Mat tmp1 = R_T2RT(CalPoseRot1, CalPoseTran1); //转移向量转旋转矩阵
	cv::Mat tmp2 = R_T2RT(CalPoseRot2, CalPoseTran2);
	cv::Mat tmp3 = R_T2RT(CalPoseRot3, CalPoseTran3);
	cv::Mat tmp4 = R_T2RT(CalPoseRot4, CalPoseTran4);
	cv::Mat tmp5 = R_T2RT(CalPoseRot5, CalPoseTran5);
	// cv::Mat tmp6 = R_T2RT(CalPoseRot6, CalPoseTran6);
	// cv::Mat tmp7 = R_T2RT(CalPoseRot7, CalPoseTran7);
	// cv::Mat tmp8 = R_T2RT(CalPoseRot8, CalPoseTran8);
 
	vecHc.push_back(tmp1);
	vecHc.push_back(tmp2);
	vecHc.push_back(tmp3);
	vecHc.push_back(tmp4);
	vecHc.push_back(tmp5);
	// vecHc.push_back(tmp6);
	// vecHc.push_back(tmp7);
	// vecHc.push_back(tmp8);
 
	//cv::Mat tmp =  R_T2RT(ToolPoseRot1, ToolPoseTran1); //转移向量转旋转矩阵
	//cv::Mat tmp9 =  R_T2RT(ToolPoseRot2, ToolPoseTran2);
	//cv::Mat tmp10 = R_T2RT(ToolPoseRot3, ToolPoseTran3);
	//cv::Mat tmp11 = R_T2RT(ToolPoseRot4, ToolPoseTran4);
	//cv::Mat tmp12 = R_T2RT(ToolPoseRot5, ToolPoseTran5);
	//cv::Mat tmp13 = R_T2RT(ToolPoseRot6, ToolPoseTran6);
	//cv::Mat tmp14 = R_T2RT(ToolPoseRot7, ToolPoseTran7);
	//cv::Mat tmp15 = R_T2RT(ToolPoseRot8, ToolPoseTran8);
	//
	//vecHg.push_back(tmp);
	//vecHg.push_back(tmp9);
	//vecHg.push_back(tmp10);
	//vecHg.push_back(tmp11);
	//vecHg.push_back(tmp12);
	//vecHg.push_back(tmp13);
	//vecHg.push_back(tmp14);
	//vecHg.push_back(tmp15);
 
	cout << "第一组和第二组手眼数据验证：" << endl;
	cout << /*vecHg[0] * */Hcg * vecHc[0] << endl << /*vecHg[1] **/ Hcg * vecHc[1] << endl << endl;//.inv()
 
	cout << "----手眼系统测试----" << endl;
	cout << "机械臂下标定板XYZ为：" << endl;
	for (int i = 0; i < vecHc.size(); ++i)
	{
		cv::Mat cheesePos{ 0.0,0.0,0.0,1.0 };//4*1矩阵，单独求机械臂下，标定板的xyz
		cv::Mat worldPos = /*vecHg[i] **/ Hcg * vecHc[i] * cheesePos;
		cout << i << ": " << worldPos.t() << endl;
	}
}


// -0.02345852660551873, 0.69403306238506, -0.7195608423517178, -0.4653030700445561;
//  0.9996920345365029, 0.01045668445287329, -0.02250541789260173, -0.01476262505702394;
//  -0.008095283427142256, -0.7198671864077905, -0.6940646225817635, 0.01335434288160803;
